#include <dalelib/maths/m3.h>

namespace dalelib
{

const m3 m3::zero(0,0,0, 0,0,0, 0,0,0);
const m3 m3::identity(1,0,0, 0,1,0, 0,0,1);

m3::m3( const float* _Ptr )
{
	for(int i=0; i<9; ++i) f[i] = _Ptr[i];
}

m3::m3( const float _Value00, const float _Value01, const float _Value02, const float _Value03, const float _Value04, const float _Value05, const float _Value06, const float _Value07, const float _Value08 )
{
	f[0] = _Value00; f[1] = _Value01; f[2] = _Value02;
	f[3] = _Value03; f[4] = _Value04; f[5] = _Value05;
	f[6] = _Value06; f[7] = _Value07; f[8] = _Value08;
}

m3::m3( const v3 _Diagonal )
{
	f[0] = _Diagonal[0]; f[1] = 0.f; f[2] = 0.f;
	f[3] = 0.f; f[4] = _Diagonal[1]; f[5] = 0.f;
	f[6] = 0.f; f[7] = 0.f; f[8] = _Diagonal[2];
}

m3::m3( const v3 _Value0, const v3 _Value1, const v3 _Value2 )
{
	f[0] = _Value0.x(); f[1] = _Value0.y(); f[2] = _Value0.z();
	f[3] = _Value1.x(); f[4] = _Value1.y(); f[5] = _Value1.z();
	f[6] = _Value2.x(); f[7] = _Value2.y(); f[8] = _Value2.z();
}
/*
const m3 m3::operator =(const m3& _Value)
{
	f[0] = _Value[0]; f[1] = _Value[1]; f[2] = _Value[2];
	f[3] = _Value[3]; f[4] = _Value[4]; f[5] = _Value[5];
	f[6] = _Value[6]; f[7] = _Value[7]; f[8] = _Value[8];
	return *this;
}
*/
//float& m3::operator[]( const int _Index ) { return f[_Index]; }

v3 m3::x() { return v3( f[0], f[1], f[2] ); }
v3 m3::y() { return v3( f[3], f[4], f[5] ); }
v3 m3::z() { return v3( f[6], f[7], f[8] ); }
v2 m3::getTranslationPart() { return v2(f[6],f[7]); }

m3 m3::operator-() const { return m3(-f[0],-f[1],-f[2],-f[3],-f[4],-f[5],-f[6],-f[7],-f[8]); }

m3 m3::transpose() const
{
	return m3(
		f[0], f[3], f[6],
		f[1], f[4], f[7],
		f[2], f[5], f[8] );
}

m3 m3::fastInverse() const
{
	// fast inverse, must be orthonormal
	return m3(
		f[ 0], f[3], f[2],
		f[ 1], f[4], f[5],
		-f[0]*f[6] - f[1]*f[7],
		-f[3]*f[6] - f[4]*f[7],
		f[8] );

}

m3 m3::inverse() const
{

	m3 m;

	// Inverse = adjoint / det. (See linear algebra texts.)

    // pre-compute 2x2 dets for last two rows when computing
    // cofactors of first two rows.
    float d12 = (f[ 2]*f[ 7] - f[ 3]*f[ 6]);
    float d13 = (f[ 2]*f[11] - f[ 3]*f[10]);
    float d23 = (f[ 6]*f[11] - f[ 7]*f[10]);
    float d24 = (f[ 6]*f[15] - f[ 7]*f[14]);
    float d34 = (f[10]*f[15] - f[11]*f[14]);
    float d41 = (f[14]*f[ 3] - f[15]*f[ 2]);

    m[0] =  (f[ 5]*d34 - f[ 9]*d24 + f[13]*d23);
    m[1] = -(f[ 1]*d34 + f[ 9]*d41 + f[13]*d13);
    m[2] =  (f[ 1]*d24 + f[ 5]*d41 + f[13]*d12);
    m[3] = -(f[ 1]*d23 - f[ 5]*d13 + f[ 9]*d12);

    // Compute determinant as early as possible using these cofactors.
    float det = f[ 0]*m[0] + f[ 4]*m[1] + f[ 8]*m[2] + f[12]*m[3];

    // Run singularity test.
    if( det == 0.0 )
    {
//        cout << "Warning: Call to invertMatrix produced a Singular matrix." << endl;
		return m3::identity;
    }
    else
    {
       float invDet = 1.f/det;

       // Compute rest of inverse.
       m[0] *= invDet;
       m[1] *= invDet;
       m[2] *= invDet;
       m[3] *= invDet;

       m[4] = -(f[ 4]*d34 - f[ 8]*d24 + f[12]*d23) * invDet;
       m[5] =  (f[ 0]*d34 + f[ 8]*d41 + f[12]*d13) * invDet;
       m[6] = -(f[ 0]*d24 + f[ 4]*d41 + f[12]*d12) * invDet;
       m[7] =  (f[ 0]*d23 - f[ 4]*d13 + f[ 8]*d12) * invDet;

       // Pre-compute 2x2 dets for first two rows when computing cofactors
       // of last two rows.
       d12 = f[ 0]*f[ 5] - f[ 1]*f[ 4];
       d13 = f[ 0]*f[ 9] - f[ 1]*f[ 8];
       d23 = f[ 4]*f[ 9] - f[ 5]*f[ 8];
       d24 = f[ 4]*f[13] - f[ 5]*f[12];
       d34 = f[ 8]*f[13] - f[ 9]*f[12];
       d41 = f[12]*f[ 1] - f[13]*f[ 0];

       m[8]  =  (f[ 7]*d34 - f[11]*d24 + f[15]*d23) * invDet;
       m[9]  = -(f[ 3]*d34 + f[11]*d41 + f[15]*d13) * invDet;
       m[10] =  (f[ 3]*d24 + f[ 7]*d41 + f[15]*d12) * invDet;
       m[11] = -(f[ 3]*d23 - f[ 7]*d13 + f[11]*d12) * invDet;
       m[12] = -(f[ 6]*d34 - f[10]*d24 + f[14]*d23) * invDet;
       m[13] =  (f[ 2]*d34 + f[10]*d41 + f[14]*d13) * invDet;
       m[14] = -(f[ 2]*d24 + f[ 6]*d41 + f[14]*d12) * invDet;
       m[15] =  (f[ 2]*d23 - f[ 6]*d13 + f[10]*d12) * invDet;

    }
    return m;

/*
	// from ogino
	Matrix mc;
	float delta;

	delta =  ma->m11*(ma->m22*ma->m33 - ma->m23*ma->m32)
		-ma->m12*(ma->m21*ma->m33 - ma->m23*ma->m31)
		+ma->m13*(ma->m21*ma->m32 - ma->m22*ma->m31);

	if(delta == 0.0){
		fprintf(stdout,
			"Error:[mInverse] Inverse Matrix does not exist.\n");
		exit(0);
	}

	mc.m11 = ( ma->m22*ma->m33 - ma->m23*ma->m32)/delta;
	mc.m21 = (-ma->m21*ma->m33 + ma->m23*ma->m31)/delta;
	mc.m31 = ( ma->m21*ma->m32 - ma->m22*ma->m31)/delta;

	mc.m12 = (-ma->m12*ma->m33 + ma->m13*ma->m32)/delta;
	mc.m22 = ( ma->m11*ma->m33 - ma->m13*ma->m31)/delta;
	mc.m32 = (-ma->m11*ma->m32 + ma->m12*ma->m31)/delta;

	mc.m13 = ( ma->m12*ma->m23 - ma->m13*ma->m22)/delta;
	mc.m23 = (-ma->m11*ma->m23 + ma->m13*ma->m21)/delta;
	mc.m33 = ( ma->m11*ma->m22 - ma->m12*ma->m21)/delta;

	return mc;
*/
}

m3 m3::noRotation() const
{
	return m3(
		1,0,f[2],
		0,1,f[5],
		f[6],f[7],f[8] );
}

m3 m3::noTranslation() const
{
	return m3(
		f[0],f[1],f[2],
		f[3],f[4],f[5],
		0,0,f[8] );
}

m3 m3::operator*( const float _Value ) const
{
	return m3(
		f[0]*_Value, f[1]*_Value, f[2]*_Value,
		f[3]*_Value, f[4]*_Value, f[5]*_Value,
		f[6]*_Value, f[7]*_Value, f[8]*_Value );
}

m3 m3::operator/( const float _Value ) const
{
	return m3(
		f[0]/_Value, f[1]/_Value, f[2]/_Value,
		f[3]/_Value, f[4]/_Value, f[5]/_Value,
		f[6]/_Value, f[7]/_Value, f[8]/_Value );
}

v3 m3::operator*( const v3& _Value ) const
{
	return v3(
		v3::dot(v3(f[0],f[3],f[6]),_Value),
		v3::dot(v3(f[1],f[4],f[7]),_Value),
		v3::dot(v3(f[2],f[5],f[8]),_Value));
}

m3 m3::operator*( const m3& _Value ) const
{
	const float* d = _Value.ptr();
	return m3(
		d[0]*f[0] + d[1]*f[3] + d[2]*f[6],
		d[0]*f[1] + d[1]*f[4] + d[2]*f[7],
		d[0]*f[2] + d[1]*f[5] + d[2]*f[8],

		d[3]*f[0] + d[4]*f[3] + d[5]*f[6],
		d[3]*f[1] + d[4]*f[4] + d[5]*f[7],
		d[3]*f[2] + d[4]*f[5] + d[5]*f[8],

		d[6]*f[0] + d[7]*f[3] + d[8]*f[6],
		d[6]*f[1] + d[7]*f[4] + d[8]*f[7],
		d[6]*f[2] + d[7]*f[5] + d[8]*f[8]);
}

m3 m3::operator+( const m3& _Value ) const
{
	const float* d = _Value.ptr();
	return m3(
		f[0]+d[0], f[1]+d[1], f[2]+d[2],
		f[3]+d[3], f[4]+d[4], f[5]+d[5],
		f[6]+d[6], f[7]+d[7], f[8]+d[8] );
}

m3 m3::operator-( const m3& _Value ) const
{
	const float* d = _Value.ptr();
	return m3(
		f[0]-d[0], f[1]-d[1], f[2]-d[2],
		f[3]-d[3], f[4]-d[4], f[5]-d[5],
		f[6]-d[6], f[7]-d[7], f[8]-d[8] );
}

void m3::operator+=(const m3& _Value)
{
	f[0] += _Value[0]; f[1] += _Value[1]; f[2] += _Value[2];
	f[3] += _Value[3]; f[4] += _Value[4]; f[5] += _Value[5];
	f[6] += _Value[6]; f[7] += _Value[7]; f[8] += _Value[8];
}

void m3::lookAt(const v2& _Pos,const v2& _Tgt)
{
	v2 x = (_Tgt-_Pos).unit();
	f[0]=x[0]; f[1]=x[1]; f[2]=0.f;
	f[3]=-x[1]; f[4]=x[0]; f[5]=0.f;
	f[6]=_Pos[0];  f[7]=_Pos[1];  f[8]=1.f;
}

void m3::print(const std::string _Str) const
{
	if( _Str!="" ) printf( "%s\n", _Str.c_str() );
	printf( "%.3f %.3f %.3f\n", 	f[0], f[3], f[6] );
	printf( "%.3f %.3f %.3f\n", 	f[1], f[4], f[7] );
	printf( "%.3f %.3f %.3f\n\n", f[2], f[5], f[8] );
}

} // namespace dalelib
